package behavior;

import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.addon.CompassHTSensor;
import lejos.robotics.subsumption.Behavior;
import motor.barredora;
import motor.carrito;
import sensor.MainColorSensorB;
import sensor.SensorColor;
import sensor.USensor;

/**
 * @version   1.0

 * @author    Bruno Argenta, Esteban Arrua, Luis Galusso
 * @copyright (C) 2012 Bruno Argenta, Esteban Arrua, Luis Galusso
 * @contact   b.argenta@gmail.com, estarr2212@gmail.com, lgalusso@gmail.com
 *
 * @license        GNU/GPLv3
 * This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with this program.  If not, see http://www.gnu.org/licenses/.
 *
 *
*/

public class ThrowOrangeBall implements Behavior{
	private SensorColor  color;   
    private int cid;
    private NXTRegulatedMotor pala;
    private carrito carro;
    private CompassHTSensor compass;
    private USensor usensor;
    private float pos = 0;
    private barredora bar;
    
    public ThrowOrangeBall(SensorColor  color, carrito carro, NXTRegulatedMotor pala, CompassHTSensor compass, USensor usensor, barredora bar ){
    	this.color = color;
    	this.carro = carro;
	    this.pala  = pala;
	    this.compass = compass;
	    this.usensor = usensor;
	    this.bar = bar;
    }

	public boolean takeControl() {
		// TODO Auto-generated method stub
		//.println("Naranja");
		
		cid = color.obtenerColor();
		//cid = color.getCurrentColor();
    	if (cid == 1){	    		
    		//.println("Tome control de THROWORANGEBALL: " +Integer.toString(cid));
    		return true;
    	}
    	return false;
	}

	public void action() {
		// TODO Auto-generated method stub
		//.println("Accion DETECTBALL");
		bar.desactivar();
    	//if(!suppressed) {
    		//if (cid == 1){  //pelota naranja
    			pala.setSpeed(1100);
    			pala.setAcceleration(6000);
    			pala.resetTachoCount();
    		//}
    		
    		pos = compass.getDegreesCartesian();
    		//carro.Backward();
    		//Delay.msDelay(1000);
    		carro.SetMovingSpeed(100);
    		carro.Spin();      	   		
    		while (!(pos > 355) || (pos < 5))    			
    			pos = compass.getDegreesCartesian();      	
        	carro.Stop();
        	carro.SetMovingSpeed(360);
        	
        	
        	// Me acerco a la pared cierta distancia
        	while(usensor.getValor() > 38) 
        		carro.Forward();
        	// Me alejo de la pared cierta distancia
        	while(usensor.getValor() < 35)
        		carro.Backward();        	
        	carro.Stop();
        	
        	
    		// Ya estoy ubicado mirando a mi NORTE y a una cierta distancia de la pared posterior,
        	// procedemos a tirar la pelota a la cancha rival
    		pala.rotate(-70,false);
			//Delay.msDelay(2000);
			pala.setAcceleration(100);
			pala.setSpeed(100);
			pala.rotate(70,false);
    	usensor.cleanDistances();
		bar.activar();
	}

	public void suppress() {	  
	} 


}
